#include "sensor_monitor_node.hpp"

namespace fishbot_sensor_monitor
{
SensorMonitorNode::SensorMonitorNode(const std::string & node_name)
: Node(node_name)
{
  // 1. 激光、IMU订阅：保持不变
  laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
    "/scan", 10, std::bind(&SensorMonitorNode::laser_callback, this, std::placeholders::_1));

  imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
    "/imu/data", 10, std::bind(&SensorMonitorNode::imu_callback, this, std::placeholders::_1));

  // 2. 修复ImageTransport构造：用 this->shared_from_this() 获取智能指针
  image_transport::ImageTransport it(this->shared_from_this());  // 关键修改：shared_from_this()

  // 3. 修复相机订阅：回调函数参数已匹配ConstSharedPtr，无需改bind逻辑
  camera_sub_ = it.subscribe(
    "/image_raw", 10, std::bind(&SensorMonitorNode::camera_callback, this, std::placeholders::_1));

  cv::namedWindow(camera_window_name_, cv::WINDOW_NORMAL);
  RCLCPP_INFO(this->get_logger(), "Sensor Monitor Node Started!");
}

// 激光、IMU回调：保持不变
void SensorMonitorNode::laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
  if (msg->ranges.empty())
  {
    RCLCPP_WARN(this->get_logger(), "Laser scan data is empty!");
    return;
  }

  float min_range = msg->range_max;
  int front_angle_start = (msg->angle_max - M_PI/18) / msg->angle_increment;
  int front_angle_end = (msg->angle_max - 17*M_PI/18) / msg->angle_increment;
  for (int i = front_angle_end; i <= front_angle_start; i++)
  {
    float range = msg->ranges[i];
    if (range > msg->range_min && range < min_range)
    {
      min_range = range;
    }
  }

  RCLCPP_INFO(
    this->get_logger(), "Laser: Front Min Distance = %.2fm (Min: %.2fm, Max: %.2fm)",
    min_range, msg->range_min, msg->range_max);
}

void SensorMonitorNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
  RCLCPP_INFO(
    this->get_logger(), "IMU: Angular Vel Z = %.2frad/s | Linear Acc X = %.2fm/s²",
    msg->angular_velocity.z, msg->linear_acceleration.x);
}

// 3. 修复相机回调：参数改为ConstSharedPtr，cv_bridge适配
void SensorMonitorNode::camera_callback(const sensor_msgs::msg::Image::ConstSharedPtr msg)
{
  try
  {
    // 关键：cv_bridge::toCvShare 支持 ConstSharedPtr，无需额外转换
    cv::Mat cv_img = cv_bridge::toCvShare(msg, "bgr8")->image;
    if (cv_img.empty())
    {
      RCLCPP_WARN(this->get_logger(), "Camera image is empty!");
      return;
    }

    cv::imshow(camera_window_name_, cv_img);
    cv::waitKey(1);
  }
  catch (cv_bridge::Exception & e)
  {
    RCLCPP_ERROR(this->get_logger(), "Camera callback error: %s", e.what());
  }
}

}  // namespace fishbot_sensor_monitor

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<fishbot_sensor_monitor::SensorMonitorNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();

  cv::destroyAllWindows();
  return 0;
}